
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       log.c
  * @author     baiyang
  * @date       2023-6-30
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"
#include "logger.h"
#include "define.h"

#include <logger/blog.h>
#include <uITC/uITC_msg.h>
#include <ahrs/ahrs_view.h>
#include <common/microbee.h>
#include <mp_control/mp_autotune.h>
#include <board_config/board_config.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
#pragma pack(1)
struct log_Control_Tuning {
    APLOG_PACKET_HEADER;
    uint64_t time_us;
    int16_t nav_roll_cd;
    int16_t roll;
    int16_t nav_pitch_cd;
    int16_t pitch;
    float throttle_out;
    float rudder_out;
    float throttle_dem;
    float airspeed_estimate;
    float synthetic_airspeed;
    float EAS2TAS;
    int32_t groundspeed_undershoot;
};

struct log_Nav_Tuning {
    APLOG_PACKET_HEADER;
    uint64_t time_us;
    float wp_distance;
    int16_t target_bearing_cd;
    int16_t nav_bearing_cd;
    int16_t altitude_error_cm;
    float   xtrack_error;
    float   xtrack_error_i;
    float   airspeed_error;
    int32_t target_lat;
    int32_t target_lng;
    int32_t target_alt_wp;
    int32_t target_alt_tecs;
    int32_t target_airspeed;
};

struct log_Status {
    APLOG_PACKET_HEADER;
    uint64_t time_us;
    uint8_t is_flying;
    float is_flying_probability;
    uint8_t armed;
    uint8_t safety;
    bool is_crashed;
    bool is_still;
    uint8_t stage;
    bool impact;
};

struct log_AETR {
    APLOG_PACKET_HEADER;
    uint64_t time_us;
    float aileron;
    float elevator;
    float throttle;
    float rudder;
    float flap;
    float steering;
    float speed_scaler;
};

struct log_OFG_Guided {
    APLOG_PACKET_HEADER;
    uint64_t time_us;
    float target_airspeed_cm;
    float target_airspeed_accel;
    float target_alt;
    float target_alt_accel;
    uint8_t target_alt_frame;
    float target_heading;
    float target_heading_limit;
};
#pragma pack()
/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
// type and unit information can be found in
// libraries/AP_Logger/Logstructure.h; search for "log_Units" for
// units and "Format characters" for field type information
static const struct LogStructure log_structure[] = {
    LOG_COMMON_STRUCTURES,

// @LoggerMessage: CTUN
// @Description: Control Tuning information
// @Field: TimeUS: Time since system startup
// @Field: NavRoll: desired roll
// @Field: Roll: achieved roll
// @Field: NavPitch: desired pitch
// @Field: Pitch: achieved pitch
// @Field: ThO: scaled output throttle
// @Field: RdrOut: scaled output rudder
// @Field: ThD: demanded speed-height-controller throttle
// @Field: As: airspeed estimate (or measurement if airspeed sensor healthy and ARSPD_USE>0)
// @Field: SAs: synthetic airspeed measurement derived from non-airspeed sensors, NaN if not available
// @Field: E2T: equivalent to true airspeed ratio
// @Field: GU: groundspeed undershoot when flying with minimum groundspeed

    { LOG_CTUN_MSG, sizeof(struct log_Control_Tuning),     
      "CTUN", "Qccccffffffi",    "TimeUS,NavRoll,Roll,NavPitch,Pitch,ThO,RdrOut,ThD,As,SAs,E2T,GU", "sdddd---nn-n", "FBBBB---00-B" , true },

// @LoggerMessage: NTUN
// @Description: Navigation Tuning information - e.g. vehicle destination
// @Field: TimeUS: Time since system startup
// @Field: Dist: distance to the current navigation waypoint
// @Field: TBrg: bearing to the current navigation waypoint
// @Field: NavBrg: the vehicle's desired heading
// @Field: AltE: difference between current vehicle height and target height
// @Field: XT: the vehicle's current distance from the current travel segment
// @Field: XTi: integration of the vehicle's crosstrack error
// @Field: AsE: difference between vehicle's airspeed and desired airspeed
// @Field: TLat: target latitude
// @Field: TLng: target longitude
// @Field: TAW: target altitude WP
// @Field: TAT: target altitude TECS
// @Field: TAsp: target airspeed
    { LOG_NTUN_MSG, sizeof(struct log_Nav_Tuning),         
      "NTUN", "QfcccfffLLeee",  "TimeUS,Dist,TBrg,NavBrg,AltE,XT,XTi,AsE,TLat,TLng,TAW,TAT,TAsp", "smddmmmnDUmmn", "F0BBB0B0GG000" , true },

// @LoggerMessage: ATRP
// @Description: Plane AutoTune
// @Vehicles: Plane
// @Field: TimeUS: Time since system startup
// @Field: Axis: tuning axis
// @Field: State: tuning state
// @Field: Sur: control surface deflection
// @Field: PSlew: P slew rate
// @Field: DSlew: D slew rate
// @Field: FF0: FF value single sample
// @Field: FF: FF value
// @Field: P: P value
// @Field: I: I value
// @Field: D: D value
// @Field: Action: action taken
// @Field: RMAX: Rate maximum
// @Field: TAU: time constant
    { LOG_ATRP_MSG, sizeof(struct log_ATRP),
      "ATRP", "QBBffffffffBff", "TimeUS,Axis,State,Sur,PSlew,DSlew,FF0,FF,P,I,D,Action,RMAX,TAU", "s#-dkk------ks", "F--00000000-00" , true },

// @LoggerMessage: STAT
// @Description: Current status of the aircraft
// @Field: TimeUS: Time since system startup
// @Field: isFlying: True if aircraft is probably flying
// @Field: isFlyProb: Probabilty that the aircraft is flying
// @Field: Armed: Arm status of the aircraft
// @Field: Safety: State of the safety switch
// @Field: Crash: True if crash is detected
// @Field: Still: True when vehicle is not moving in any axis
// @Field: Stage: Current stage of the flight
// @Field: Hit: True if impact is detected
    { LOG_STATUS_MSG, sizeof(struct log_Status),
      "STAT", "QBfBBBBBB",  "TimeUS,isFlying,isFlyProb,Armed,Safety,Crash,Still,Stage,Hit", "s--------", "F--------" , true },

// @LoggerMessage: QTUN
// @Description: QuadPlane vertical tuning message
// @Field: TimeUS: Time since system startup
// @Field: ThI: throttle input
// @Field: ABst: angle boost
// @Field: ThO: throttle output
// @Field: ThH: calculated hover throttle
// @Field: DAlt: desired altitude
// @Field: Alt: achieved altitude
// @Field: BAlt: barometric altitude
// @Field: DCRt: desired climb rate
// @Field: CRt: climb rate
// @Field: TMix: transition throttle mix value
// @Field: Sscl: speed scalar for tailsitter control surfaces
// @Field: Trn: Transistion state
// @Field: Ast: Q assist active state
#if HAL_QUADPLANE_ENABLED
    { LOG_QTUN_MSG, sizeof(QuadPlane::log_QControl_Tuning),
      "QTUN", "QffffffeccffBB", "TimeUS,ThI,ABst,ThO,ThH,DAlt,Alt,BAlt,DCRt,CRt,TMix,Sscl,Trn,Ast", "s----mmmnn----", "F----00000-0--" , true },
#endif

// @LoggerMessage: PIQR,PIQP,PIQY,PIQA
// @Description: QuadPlane Proportional/Integral/Derivative gain values for Roll/Pitch/Yaw/Z
// @Field: TimeUS: Time since system startup
// @Field: Tar: desired value
// @Field: Act: achieved value
// @Field: Err: error between target and achieved
// @Field: P: proportional part of PID
// @Field: I: integral part of PID
// @Field: D: derivative part of PID
// @Field: FF: controller feed-forward portion of response
// @Field: Dmod: scaler applied to D gain to reduce limit cycling
// @Field: SRate: slew rate
// @Field: Limit: 1 if I term is limited due to output saturation
    { LOG_PIQR_MSG, sizeof(struct log_PID),
      "PIQR", PID_FMT,  PID_LABELS, PID_UNITS, PID_MULTS , true },
    { LOG_PIQP_MSG, sizeof(struct log_PID),
      "PIQP", PID_FMT,  PID_LABELS, PID_UNITS, PID_MULTS , true },
    { LOG_PIQY_MSG, sizeof(struct log_PID),
      "PIQY", PID_FMT,  PID_LABELS, PID_UNITS, PID_MULTS , true },
    { LOG_PIQA_MSG, sizeof(struct log_PID),
      "PIQA", PID_FMT,  PID_LABELS, PID_UNITS, PID_MULTS , true },

// @LoggerMessage: PIDG
// @Description: Plane Proportional/Integral/Derivative gain values for Heading when using COMMAND_INT control.
// @Field: TimeUS: Time since system startup
// @Field: Tar: desired value
// @Field: Act: achieved value
// @Field: Err: error between target and achieved
// @Field: P: proportional part of PID
// @Field: I: integral part of PID
// @Field: D: derivative part of PID
// @Field: FF: controller feed-forward portion of response
// @Field: Dmod: scaler applied to D gain to reduce limit cycling
// @Field: SRate: slew rate
// @Field: Limit: 1 if I term is limited due to output saturation
    { LOG_PIDG_MSG, sizeof(struct log_PID),
      "PIDG", PID_FMT,  PID_LABELS, PID_UNITS, PID_MULTS , true },

// @LoggerMessage: AETR
// @Description: Normalised pre-mixer control surface outputs
// @Field: TimeUS: Time since system startup
// @Field: Ail: Pre-mixer value for aileron output (between -4500 to 4500)
// @Field: Elev: Pre-mixer value for elevator output (between -4500 to 4500)
// @Field: Thr: Pre-mixer value for throttle output (between -4500 to 4500)
// @Field: Rudd: Pre-mixer value for rudder output (between -4500 to 4500)
// @Field: Flap: Pre-mixer value for flaps output (between -4500 to 4500)
// @Field: Steer: Pre-mixer value for steering output (between -4500 and 4500)
// @Field: SS: Surface movement / airspeed scaling value
    { LOG_AETR_MSG, sizeof(struct log_AETR),
      "AETR", "Qfffffff",  "TimeUS,Ail,Elev,Thr,Rudd,Flap,Steer,SS", "s-------", "F-------" , true },

// @LoggerMessage: OFG
// @Description: OFfboard-Guided - an advanced version of GUIDED for companion computers that includes rate/s.  
// @Field: TimeUS: Time since system startup
// @Field: Arsp:  target airspeed cm
// @Field: ArspA:  target airspeed accel
// @Field: Alt:  target alt
// @Field: AltA: target alt accel
// @Field: AltF: target alt frame
// @Field: Hdg:  target heading
// @Field: HdgA: target heading lim
    { LOG_OFG_MSG, sizeof(struct log_OFG_Guided),     
      "OFG", "QffffBff",    "TimeUS,Arsp,ArspA,Alt,AltA,AltF,Hdg,HdgA", "s-------", "F-------" , true }, 
};

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// Write an attitude packet
void aplog_write_plane_attitude()
{
    uitc_vehicle_attitude vehicle_attitude;
    uitc_sensor_gyr sensor_gyr;

    Vector3f_t targets;       // Package up the targets into a vector for commonality with Copter usage of Log_Wrote_Attitude
    targets.x = fms.nav_roll_cd;
    targets.y = fms.nav_pitch_cd;
    targets.z = 0; //Plane does not have the concept of navyaw. This is a placeholder.

    if (itc_copy_from_hub(ITC_ID(vehicle_attitude), &vehicle_attitude) == 0 && \
        itc_copy_from_hub(ITC_ID(sensor_gyr), &sensor_gyr) == 0) {
        const struct log_Attitude pkt = {
            APLOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG),
            .time_us         = vehicle_attitude.timestamp_us,
            .control_roll    = (int16_t)targets.x,
            .roll            = (int16_t)degrees(vehicle_attitude.vehicle_euler.roll * 100),
            .control_pitch   = (int16_t)targets.y,
            .pitch           = (int16_t)degrees(vehicle_attitude.vehicle_euler.pitch * 100),
            .control_yaw     = (uint16_t)targets.z,
            .yaw             = (uint16_t)math_wrap_360_cd(degrees(vehicle_attitude.vehicle_euler.yaw * 100)),
            .error_rp        = 0,
            .error_yaw       = 0,
            .active          = 0,
        };
        blog_write_critical_block(&pkt, sizeof(pkt));
    }

    aplog_write_pid(LOG_PIDR_MSG, rctrl_get_pid_info(&fms.roll_controller));
    aplog_write_pid(LOG_PIDP_MSG, pctrl_get_pid_info(&fms.pitch_controller));

    if (yctrl_enabled(&fms.yaw_controller)) {
        aplog_write_pid(LOG_PIDY_MSG, yctrl_get_pid_info(&fms.yaw_controller));
    }

    if (sctrl_active(&fms.steer_controller)) {
        aplog_write_pid(LOG_PIDS_MSG, sctrl_get_pid_info(&fms.steer_controller));
    }
}

// Write a control tuning packet. Total length : 22 bytes
void aplog_write_plane_control_tuning()
{
    float est_airspeed = 0;
    ahrs_airspeed_estimate(fms.ahrs, &est_airspeed);

    float synthetic_airspeed = aplog_quiet_nanf();

#if 0
    if (!ahrs.synthetic_airspeed(synthetic_airspeed)) {
        synthetic_airspeed = logger.quiet_nan();
    }
#endif

    struct log_Control_Tuning pkt = {
        APLOG_PACKET_HEADER_INIT(LOG_CTUN_MSG),
        .time_us         = time_micros64(),
        .nav_roll_cd     = (int16_t)fms.nav_roll_cd,
        .roll            = (int16_t)fms.ahrs->roll_sensor_cd,
        .nav_pitch_cd    = (int16_t)fms.nav_pitch_cd,
        .pitch           = (int16_t)fms.ahrs->pitch_sensor_cd,
        .throttle_out    = srv_channels_get_output_scaled(k_throttle),
        .rudder_out      = srv_channels_get_output_scaled(k_rudder),
        .throttle_dem    = aplog_quiet_nanf(),
        .airspeed_estimate = est_airspeed,
        .synthetic_airspeed = synthetic_airspeed,
        .EAS2TAS            = ahrs_get_EAS2TAS(fms.ahrs),
        .groundspeed_undershoot  = fms.groundspeed_undershoot,
    };
    blog_write_critical_block(&pkt, sizeof(pkt));
}

void aplog_write_plane_status()
{
    struct log_Status pkt = {
        APLOG_PACKET_HEADER_INIT(LOG_STATUS_MSG),
        .time_us   = time_micros64(),
        .is_flying   = fms_is_flying(),
        .is_flying_probability = fms.isFlyingProbability,
        .armed       = brd_get_soft_armed(),
        .safety      = (uint8_t)(brd_safety_switch_state()),
        .is_crashed  = fms.crash_state.is_crashed,
        .is_still    = fms.ahrs->ins_is_still,
        .stage       = (uint8_t)(fms.flight_stage),
        .impact      = fms.crash_state.impact_detected,
        };

    blog_write_critical_block(&pkt, sizeof(pkt));
}

void aplog_write_plane_AETR()
{
    struct log_AETR pkt = {
        APLOG_PACKET_HEADER_INIT(LOG_AETR_MSG),
        .time_us  = time_micros64(),
        .aileron  = srv_channels_get_output_scaled(k_aileron),
        .elevator = srv_channels_get_output_scaled(k_elevator),
        .throttle = srv_channels_get_output_scaled(k_throttle),
        .rudder   = srv_channels_get_output_scaled(k_rudder),
        .flap     = srv_channels_get_slew_limited_output_scaled(k_flap_auto),
        .steering     = srv_channels_get_output_scaled(k_steering),
        .speed_scaler = fms.surface_speed_scaler,
        };

    blog_write_critical_block(&pkt, sizeof(pkt));
}

// 注意不要在plane_log_init中调用
void aplog_write_vehicle_startup_messages()
{
    // only 200(?) bytes are guaranteed by AP_Logger
    aplog_write_messageF("MicrobeePlane V%d.%d.%d-dev",FIRMWARE_VERSION);

    aplog_write_mode((uint8_t)mode_number(fms.control_mode), fms.control_mode_reason);
    ahrs_log_write_home_and_origin(fms.ahrs);
    //gps.Write_AP_Logger_Log_Startup_messages();
}

int plane_log_init(void)
{
    aplog_init(&log_structure[0], ARRAY_SIZE(log_structure));
    return 0;
}
MB_INIT_COMPONENT_EXPORT(plane_log_init);
/*------------------------------------test------------------------------------*/


